/*
 * Copyright (C) 2023 ab_skywalker@163.com All Rights Reserved.
 *
 * SPDX-License-Identifier: MIT
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of
 * this software and associated documentation files (the "Software"), to deal in
 * the Software without restriction, including without limitation the rights to
 * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
 * the Software, and to permit persons to whom the Software is furnished to do so,
 * subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
 * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
 * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
 * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 *
 */
#include "motortask.h"
#include "FreeRTOS.h"
#include "event_groups.h"
#include "drv_ad.h"
#include "drv_time.h"
#include "drv_encoder.h"
#include "foc_model.h"
#include "foc_para.h"
#include "foc_velocity.h"
#include "foc_position.h"
#include "foc_tune.h"
#include <stdio.h>

#define MOTOR_VELOCITY_PERIOD 10
#define MOTOR_POSITION_PERIOD 100

EventGroupHandle_t MotorEvent;
TimerHandle_t MotorVelocityTimerHdl;
TimerHandle_t MotorPositionTimerHdl;

void motorCurrentLoopCallback(int32_t a, int32_t b, int32_t c)
{
    int32_t ia = drvAdCurrCompute(a, 0);
    int32_t ib = drvAdCurrCompute(b, 1);
    int32_t ic = drvAdCurrCompute(c, 2);
    focModel(ia, ib, ic);
}

void motorVelocityLoopCallback(TimerHandle_t xTimer)
{
    //注册速度环控制函数
    focVelocityLoop(MOTOR_VELOCITY_PERIOD);
}

void motorPositionLoopCallback(TimerHandle_t xTimer)
{
    //注册位置环控制函数
    focPositionLoop(MOTOR_POSITION_PERIOD);
}

void motorRotateCtrl(bool ctrl)
{
    //PWM启停控制
    ctrl == true ? startPwm() : stopPwm();
}

void motorLocateCtrl(void)
{
    //编码器校零
    drvAdCurrCallbackRegister(NULL);//暂时屏蔽FOC运行，直接控制PWM输出
    focLocateCtrl();
    drvAdCurrCallbackRegister(motorCurrentLoopCallback);//还原FOC控制
}

void motorComputeCurrBias(void)
{
    //令PWM占空比为0，上桥臂关闭下桥臂打开，此时相电流等于静态偏置电流
    motorRotateCtrl(false);
    updataPwmDuty(0, 0, 0);
    drvAdCurrCallbackRegister(drvAdCurrBiasCallback);
    motorRotateCtrl(true);
    msleep(100);
    motorRotateCtrl(false);
    drvAdCurrComputeBias();
    drvAdCurrCallbackRegister(motorCurrentLoopCallback);
}

void motorTask(void * argument)
{
    EventBits_t motorbit;
    MotorEvent = xEventGroupCreate();
    motorParaInit();
    adInit();
    advTimeInit();
    encoderInit();
    focInit();
    focTuneRealtimeDebug(true);
    motorComputeCurrBias();
    motorLocateCtrl();

    focVelocityInit();
    focPositionInit();
    motorRotateCtrl(true);

    MotorVelocityTimerHdl = xTimerCreate("", pdMS_TO_TICKS(MOTOR_VELOCITY_PERIOD), pdTRUE, ( void * ) NULL, motorVelocityLoopCallback);
    xTimerStart(MotorVelocityTimerHdl, 0);
    MotorPositionTimerHdl = xTimerCreate("", pdMS_TO_TICKS(MOTOR_POSITION_PERIOD), pdTRUE, ( void * ) NULL, motorPositionLoopCallback);
    xTimerStart(MotorPositionTimerHdl, 0);
    while(1) {
        motorbit = xEventGroupWaitBits(MotorEvent, \
            MOTOR_CTRL_POWEROFF | \
            MOTOR_CTRL_POWERON | \
            MOTOR_CTRL_ROTATE | \
            MOTOR_CTRL_STOP, \
            pdTRUE, pdFALSE, portMAX_DELAY);
        if(motorbit & MOTOR_CTRL_ROTATE != 0) {
            motorRotateCtrl(true);
        }
        if(motorbit & MOTOR_CTRL_POWERON != 0) {
            adInit();
            advTimeInit();
            encoderInit();
            focInit();
        }
        if(motorbit & MOTOR_CTRL_STOP != 0) {
            motorRotateCtrl(false);
        }
        if(motorbit & MOTOR_CTRL_POWEROFF != 0) {
            advTimeDeinit();
            adDeinit();
            encoderDeinit();
        }
    }
}

void motorCtrl(MOTOR_CTRL_E ctrl)
{
    switch(ctrl) {
        case MOTOR_CTRL_POWEROFF:
        case MOTOR_CTRL_POWERON:
        case MOTOR_CTRL_ROTATE:
        case MOTOR_CTRL_STOP:
            xEventGroupSetBits(MotorEvent, ctrl);
            break;
        default:
            break;
    }
}